
/**
 * From QT OpenGL demo: boxes
 */


#include "trackball.h"

M3D_BEGIN_NAMESPACE

TrackBall::TrackBall()
    : m_angularVelocity(0)
    , m_paused(false)
    , m_pressed(false)
{
    m_axis = MVector3D(0, 1, 0);
    m_rotation = MQuaternion();
    m_lastTime = MTime::currentTime();
}

TrackBall::TrackBall(float angularVelocity, const MVector3D& axis)
    : m_axis(axis)
    , m_angularVelocity(angularVelocity)
    , m_paused(false)
    , m_pressed(false)
{
    m_rotation = MQuaternion();
    m_lastTime = MTime::currentTime();
}

void TrackBall::push(const MPointF& p)
{
    m_rotation = rotation();
    m_pressed = true;
    m_lastTime = MTime::currentTime();
    m_lastPos = p;
    m_angularVelocity = 0.0f;
}

void TrackBall::move(const MPointF& p)
{

    if (!m_pressed)
        return;

    MTime currentTime = MTime::currentTime();
    int msecs = m_lastTime.msecsTo(currentTime);
    if (msecs <= 20)
        return;


    MVector3D lastPos3D = MVector3D(m_lastPos.x(), m_lastPos.y(), 0.0f);
    float sqrZ = 1 - MVector3D::dotProduct(lastPos3D, lastPos3D);
    if (sqrZ > 0)
        lastPos3D.setZ(mSqrt(sqrZ));
    else
        lastPos3D.normalize();

    MVector3D currentPos3D = MVector3D(p.x(), p.y(), 0.0f);
    sqrZ = 1 - MVector3D::dotProduct(currentPos3D, currentPos3D);
    if (sqrZ > 0)
        currentPos3D.setZ(mSqrt(sqrZ));
    else
        currentPos3D.normalize();

    m_axis = MVector3D::crossProduct(lastPos3D, currentPos3D);
    float angle = 180 / PI * mAsin(mSqrt(MVector3D::dotProduct(m_axis, m_axis)));
	m_angularVelocity = angle / msecs;
    m_axis.normalize();
    m_rotation = MQuaternion::fromAxisAndAngle(m_axis, angle) * m_rotation;

    m_lastPos = p;
    m_lastTime = currentTime;

}

void TrackBall::release(const MPointF& p)
{
    m_pressed = false;
}

void TrackBall::start()
{
    m_lastTime = MTime::currentTime();
    m_paused = false;
}

void TrackBall::stop()
{
    m_rotation = rotation();
    m_paused = true;
}   


MQuaternion TrackBall::rotation() const
{

//    return m_rotation;

    if (m_paused || m_pressed)
        return m_rotation;

    MTime currentTime = MTime::currentTime();
    float angle = m_angularVelocity * m_lastTime.msecsTo(currentTime);
    return MQuaternion::fromAxisAndAngle(m_axis, angle) * m_rotation;

}

M3D_END_NAMESPACE
